
#include "User_define.h"
#include "DSP280x_Device.h"					// Peripheral address definitions
#include "f2808_HRPWM_DAC.h"				// Include file specific to DAC
#include "DSP280x_Examples.h"   			// DSP280x Examples Include File
#include "Example_posspeed.h"   			// Example specific Include file


#include <stdio.h>
#include <math.h>
#include <stdlib.h>


//void integral(double *,double);
//void ComplentaryFilter(double *,double,double);
double nonlinear(double, double[]);
double nonlinear_l(double, double[]);
interrupt void cpu_timer0_isr(void);
void Gyro_getValues(double *, int );
void Get_RollPitch(double *, int );
//void MyIMU_AHRSupdate(double *,double *,double *);
//void MPU_getMotion6(float *,float *,float *,float *,float *,float *);
//unsigned long millis();



// Global variables used in this example:
int16 MEP_ScaleFactor[5] = {0,0,0,0,0}; 	// HRPWM Scale Factor values for ePWM1-4

float32 us = 0.0 ;			    			//Global Definition of Input for Using ISR
float32 ul = 0.0 ;

//dung duy nhat timer0 cho chuong trinh quad
//unsigned long timer0_millis = 0;
//unsigned long now = 0 ;
//unsigned long last =0;
double theta_dot[3]={0.0,0.0,0.0};
double RollPitch[2]= {0.0,0.0};

/*
double theta_3_dot = 0.0;
double theta_1_dot = 0.0;
double theta_2_dot = 0.0;

double RAccx=0.0;
double RAccy=0.0;
double RAccz=0.0;
double R_x =0.0;
double R_y =0.0;

   
float gOffsets_x=0.0;
float gOffsets_y=0.0;
float gOffsets_z=0.0;
*/
 
POSSPEED theta_2=POSSPEED_DEFAULTS;
POSSPEED phi=POSSPEED_DEFAULTS;

double xs[NUMBER_OF_FACTOR] = {0.0, 0.0, 0.0, 0.0};
double xl[NUMBER_OF_FACTOR] = {0.0, 0.0, 0.0, 0.0};

double u_control=0.0;

//disturbance

//double position_p=0.0;
//double position_r=0.0;
//double position_y=0.0;


//double speed = 0.0;
//output data
/*
int data_u[POINT] = {0.0};
int xhs0[POINT] ={0.0};
int xhs1[POINT] ={0.0};
int xhs2[POINT] ={0.0};


//Counter
Uint16 Counter_jairo_1 = 0 ;
Uint16 Counter_jairo_2 = 0 ;
*/
//Encoder rotation
int roll = 0 ;
int roll_l = 0 ;

//input
Uint16 DAC1_frac, DAC2_frac;

// Global Variables used by HRPWM SFO functions
//
volatile struct EPWM_REGS *ePWM[] = {&EPwm1Regs, &EPwm2Regs, &EPwm3Regs,
                                     &EPwm4Regs, &EPwm5Regs, &EPwm6Regs};

void main(void){

  
 
    //*** CPU Initialization
  InitSysCtrl();
  							// Initialize the CPU (FILE: SysCtrl.c)
  EALLOW;
  SysCtrlRegs.HISPCP.all = ADC_MODCLK;	// HSPCLK = SYSCLKOUT/ADC_MODCLK
  EDIS;
  
  InitGpio();								// Initialize the shared GPIO pins (FILE: Gpio.c)
 // InitEQep1Gpio();

  DINT;								  // Clear all interrupts and initialize PIE vector table: Disable CPU interrupts 

  InitPieCtrl();							// Initialize and enable the PIE (FILE: PieCtrl.c)
  
  // Disable CPU interrupts and clear all CPU interrupt flags:
   IER = 0x0000;
   IFR = 0x0000;

  InitPieVectTable();// Initialize the PIE vector table with pointers to the shell Interrupt Service Routines (ISR).  


   EALLOW;  // This is needed to write to EALLOW protected registers
   PieVectTable.TINT0 = &cpu_timer0_isr;
   EDIS;  // This is needed to disable write to EALLOW protected registers
 
  
  InitCpuTimers();
//	EPwmSetup();
  //InitPeripherals(); // Not required for this example
   //InitAdc();  // For this example, init the ADC

//*** PWM/DAC Initialization

// Do the initial HRPWM calibration.
//
// In this example, one ePWM HRPWM module is being used as the calibrator,
// under the assumption that the other ePWM modules are all similar.  For
// more information, see the "TMS320x280x High-Resolution Pulse Width
// Modulator (HRPWM) Reference Guide," Literature #SPRU924.
	while ( MEP_ScaleFactor[HRPWM_CAL_CH] == 0 ) SFO_MepDis(HRPWM_CAL_CH);

// Setup the ePWM for the DAC function
	InitDac(DAC1_pin, DAC1_period);
	InitDac(DAC2_pin, DAC2_period);

//*** Enable the clocks to the ePWM module
// Note: this should be done after all ePWM modules are configured to
// ensure synchronization between the ePWM modules.
	asm(" EALLOW");							// Enable EALLOW protected register access
	SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1;	// HSPCLK to ePWM modules enabled
	asm(" EDIS");							// Disable EALLOW protected register access

// This function is found in DSP280x_InitPeripherals.c
// InitPeripherals(); // Not required for this example
   InitAdc();  // For this example, init the ADC
     
// Specific ADC setup for this example:
  // AdcRegs.ADCTRL1.bit.ACQ_PS = ADC_SHCLK;  //  
  // AdcRegs.ADCTRL3.bit.ADCCLKPS = ADC_CKPS; //   
  // AdcRegs.ADCTRL1.bit.SEQ_CASC = 1;        // 1  Cascaded mode
  // AdcRegs.ADCMAXCONV.bit.MAX_CONV1 = 0x0; // convert and store in 3 results registers 
  // AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0x1;
 //  AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x1;
//   AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x11;
  // AdcRegs.ADCTRL1.bit.CONT_RUN = 1;       // Setup continuous run 
  // AdcRegs.ADCMAXCONV.bit.MAX_CONV1 = 0x0; // convert and store in 3 results registers

    DAC1_frac = 1.4 / 3.3 * Q15_SCALE;
	DAC2_frac = 1.4 / 3.3 * Q15_SCALE;
	//DAC2_frac = 1.4 / 3.3 * Q15_SCALE;
	WriteDac(DAC1_pin, DAC1_frac);//Red Code
	WriteDac(DAC2_pin, DAC2_frac);//Brown code

// Configure QEP


	InitEQepGpio();      //BreakPoint for Experiment

//*** Configure CPU Timer0
//	InitTimer0();
//*** Configure CPU Timer0^-	
   //ConfigCpuTimer(&CpuTimer0, 100e-4, 1000000);
   ConfigCpuTimer(&CpuTimer0, 100, 100000) ;
   StartCpuTimer0();
   //InitTimer0();
    
  //  EPwmSetup();
//*** Enable interrupts
    SetDBGIER(IER);							// Configure the DBGIER for realtime debug
   asm(" CLRC INTM, DBGM");				// Enable global interrupts and realtime debug

   PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
   IER |= M_INT1;
 //Enable CPU INT1 which is connected to CPU-Timer 0:
   PieCtrlRegs.PIEIER3.bit.INTx1 = 1;
   IER |= M_INT3;
  
   EINT;          // Enable Global interrupt INTM
   ERTM;          // Enable Global realtime interrupt DBGM
   
   
   POSSPEED_Init();
       AdcRegs.ADCTRL1.bit.ACQ_PS = 4;
   AdcRegs.ADCTRL1.bit.CPS = 1;
   AdcRegs.ADCTRL3.bit.SMODE_SEL = 1;
   AdcRegs.ADCTRL1.bit.SEQ_CASC = 1;
   AdcRegs.ADCMAXCONV.all = 0x0002;       // Setup 2 conv's on SEQ1 (OK)
//   AdcRegs.ADCMAXCONV.all = 0x0001;
   AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0x0; // Setup ADCINA3 as 1st SEQ1 conv.
   AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x2; // Setup ADCINA0 as 1st SEQ1 conv.
   AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 0x4;
   AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 0x6;//test
   //AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 0x4; // Setup ADCINA2 as 2nd SEQ1 conv.
   AdcRegs.ADCTRL2.bit.EPWM_SOCA_SEQ1 = 1;// Enable SOCA from ePWM to start SEQ1
   AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1;  // Enable SEQ1 interrupt (every EOS)

// Assumes ePWM1 clock is already enabled in InitSysCtrl();
   EPwm1Regs.ETSEL.bit.SOCAEN = 1;        // Enable SOC on A group
   EPwm1Regs.ETSEL.bit.SOCASEL = 4;       // Select SOC from from CPMA on upcount
   EPwm1Regs.ETPS.bit.SOCAPRD = 1;        // Generate pulse on 1st event 
  // EPwm1Regs.CMPA.half.CMPA = 0x0080;	  // Set compare A value(128)(OK)
   EPwm1Regs.CMPA.half.CMPA = 0x0080;
   EPwm1Regs.TBPRD = 0x1FFE; // Set period for ePWM1(65535)(OK)
  // EPwm1Regs.TBPRD = 0x4999; //test            
   EPwm1Regs.TBCTL.bit.CTRMODE = 0;		  // count up and start

  //SFO_MepDis(HRPWM_CAL_CH);
   
    			// format motor
 //  last = millis();
	
//*** Main Loop
	while(1)
{

 
    POSSPEED_Calc(&theta_2, &phi);

/*	
    MyIMU_AHRSupdate(&position_p,&position_r,&position_y);
	MPU_getMotion6(&val[0],&val[1],&val[2],&val[3],&val[4],&val[5]);
	now = timer0_millis;
	while (now - last < 50) now = millis();
	last = now;	
*/
    	


  	xs[1] = theta_dot[0] ;
  	xs[0] = RollPitch[0];
  	xs[3] = R * ( -(double)(phi.SpeedRpm_pr)/( 60.0 * (double)GEAR_RATIO )   * PI); 
  	xs[2] = R * ( -((phi.theta_raw) / (double)ENCORDER_SCALE + (double)roll) * 2.0 * PI / (double)GEAR_RATIO);
	


	xl[3] = (-(double)(theta_2.SpeedRpm_pr)/( 60.0 * (GEAR_RATIO) ) * PI);
	xl[2] = (-((theta_2.theta_raw) / (double)ENCORDER_SCALE + (double)roll_l) * 2.0 * PI / ((double)GEAR_RATIO));
    xl[1] =  theta_dot[1];
	xl[0] =  RollPitch[1];

    us = nonlinear(u_control,xs);
	ul = nonlinear_l(u_control,xl);
	inputConverterLateral(&DAC1_frac, &ul);//OK
	inputConverterSagittal(&DAC2_frac, &us);//OK
    WriteDac(DAC1_pin, DAC1_frac);	
    WriteDac(DAC2_pin, DAC2_frac);

	// thoi gian 50ms (20Hz) -> co the thay halfT = 25; trong AHRS proc
	

    
									// Dummy loop.  Wait for an interrupt.
	}

}

interrupt void cpu_timer0_isr(void)
{

  
    Gyro_getValues(theta_dot, 3);
	Get_RollPitch(RollPitch, 2)


	//CpuTimer0.InterruptCount++;
    //Counter_jairo_1 &= 0xFF;
    // Acknowledge this interrupt to receive more interrupts from group 1
   AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1;         // Reset SEQ1
   AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;       // Clear INT SEQ1 bit
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}



/*** end of file *****************************************************/
